import math
import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node, PushROSNamespace
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    创建一个带有两个使用外部控制逻辑的机器人仿真环境
    为每个机器人配置一个外部控制器和轨迹跟踪器
    创建局部路径规划器，生成局部轨迹
    """

    webotsSim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution(
                [FindPackageShare("robot_sim"), "launch", "sim_direct.launch.py"]
            )
        )
    )

    core = GroupAction(
        actions=[
            PushROSNamespace("robot1"),
            IncludeLaunchDescription(
                PythonLaunchDescriptionSource(
                    PathJoinSubstitution(
                        [FindPackageShare("core"), "launch", "core.launch.py"]
                    )
                ),
                launch_arguments={
                    "prefix": "/robot1",
                }.items(),
            ),
        ]
    )

    inputJS = Node(
        package="input",
        executable="joystick",
        name="js_input",
        output="screen",
    )

    return LaunchDescription([webotsSim, inputJS, core])
